#include "dds_test_tool/dds_test_subscriber.h"
#include "global_defines.h"

#include <numeric>

DDSTestSubscriber::DDSTestSubscriber(bool isRealiable, int historyDepth) :
    Node("dds_test_sub")
{

    rclcpp::QoS profile(historyDepth);
    if(isRealiable){
        profile.reliability(RMW_QOS_POLICY_RELIABILITY_RELIABLE);
    }else{
        profile.reliability(RMW_QOS_POLICY_RELIABILITY_BEST_EFFORT);
    }

    dataSub = create_subscription<dds_test_interface::msg::TestData>(
        DDS_TEST_TOPIC,
        profile,
        std::bind(&DDSTestSubscriber::onDDSTestDataReceived, this, std::placeholders::_1)
    );
}

DDSTestSubscriber::~DDSTestSubscriber()
{

}

void DDSTestSubscriber::onDDSTestDataReceived(dds_test_interface::msg::TestData::SharedPtr msg)
{

    rclcpp::Time now = get_clock()->now();
    if(statisticsVec.size() == 0){
        statisticsStTs = now.seconds();
    }

    double deltaSec = now.seconds() - (msg->header.stamp.sec  + msg->header.stamp.nanosec * 1e-9);
    statisticsVec.push_back(deltaSec);


    if(now.seconds() - statisticsStTs >= 1.0 && statisticsVec.size() != 0){
        double avg = std::accumulate(statisticsVec.begin(), statisticsVec.end(), 0.0) / statisticsVec.size();
        RCLCPP_INFO(get_logger(), "Delay nanosec : %f, %ld", avg * 1e9, statisticsVec.size());

        statisticsVec.clear();
    }

    if(rcvCnt == 0 || msg->msg_cnt == 0){
        rcvCnt = msg->msg_cnt;
    }else if(rcvCnt != msg->msg_cnt){
        RCLCPP_WARN(get_logger(), "Message Frame Missing %d, %d", rcvCnt, msg->msg_cnt);
        rcvCnt = msg->msg_cnt;
    }
   
   rcvCnt++;
}